#include<iostream>
#include<string>
#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/calib3d.hpp>

using namespace std;
using namespace cv;

void setconfig(rs2::config &cfg,rs2::pipeline &pipe)
{
    cfg.enable_stream(RS2_STREAM_FISHEYE,1, RS2_FORMAT_Y8);
    cfg.enable_stream(RS2_STREAM_FISHEYE,2, RS2_FORMAT_Y8);
    pipe.start(cfg);
}

void getimageinst(rs2::pipeline_profile profile,rs2_intrinsics distparam,rs2_stream streamtype,int index,Mat &K,Mat &D)
{
    distparam=profile.get_stream(streamtype,index).as<rs2::video_stream_profile>().get_intrinsics();
    double k[3][3]={distparam.fx,0,distparam.ppx,0,distparam.fy,distparam.ppy,0,0,1};

    
    double d[4]={distparam.coeffs[0],distparam.coeffs[1],distparam.coeffs[2],distparam.coeffs[3]};
    Mat(3,3,CV_64F,k).copyTo(K);
    D=Mat(1,4,CV_64F,d);
}

Mat GetImage(rs2::frameset data,int index)
{
    rs2::frame image = data.get_fisheye_frame(index);
  
    if (!image) {
        cout<<"can't get the image"<<endl;
    }

     Mat src=Mat(cv::Size(848, 800), CV_8U, (void*)image.get_data(), cv::Mat::AUTO_STEP);
     return src;
}
    
    
uchar interplot(const Mat &scr,const Point &p)
{
    int xx=floor(p.x);
    int yy=floor(p.y);
    uchar p00=scr.at<uchar>(xx,yy);
    uchar p01=scr.at<uchar>(xx,yy+1);
    uchar p10=scr.at<uchar>(xx+1,yy);
    uchar p11=scr.at<uchar>(xx+1,yy+1);
    
    double u=p.x-xx;
    double v=p.y-yy;
    return (1-u)*(1-v)*p00+(1-u)*(v)*p01+u*(1-v)*p10+v*u*p11;
}
    
void undistortself(Mat src, Mat &dst, Mat K, Mat D,)
{
    int rows =src.rows, cols =src.cols;
    for(int u=0;u<rows;u++){
        for(int v=0;v<cols;v++){
            double x1,y1,x2,y2;
            double x1=(u-K.at<CV_32F>(0,2))/K.at<CV_32F>(0,0);
            double y1=(v-K.at<CV_32F>(1,2))/K.at<CV_32F>(1,1);
            double r2=x*x+y*y;
            double x2=x1*(1+D.at<CV_32F>(0,0)*r1+D.at<CV_32F>(0,1)*r2*r2)+2*D.at<CV_32F>(0,1)*x1*y1+D.at<CV_32F>(0,2)*(r2+2*x1*x1);
            double y2= y1*(1+D.at<CV_32F>(0,0)*r2+D.at<CV_32F>(0,1)*pow(r2,2))+D.at<CV_32F>(0,2)*(r2+2*y1*y1)+2*D.at<CV_32F>(0,3)*x1*y1;
            double u_distorted = K.at<CV_32F>(0,0)*x2+K.at<CV_32F>(0,2);
            double v_distorted = K.at<CV_32F>(1,1)*y2+K.at<CV_32F>(1,2);
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
                dst.at<uchar>(v, u) = interplot(src,Point(v_distorted,u_distorted));
            } else {
                dst.at<uchar>(v, u) = 0;
            }
        }
    }
}
    
int main(int argc,char** argv)
{
     rs2::config cfg;
     rs2::pipeline pipe;
     setconfig(cfg,pipe);
    
     rs2::frameset data;
     rs2::stream_profile profiles;
     rs2::pipeline_profile profile=pipe.get_active_profile(); 
     
     Mat Kl(3,3,CV_64F),Dl(4,1,CV_64F),Kr(3,3,CV_64F),Dr(4,1,CV_64F);
     rs2_intrinsics distparaml,distparamr;
     getimageinst(profile,distparaml,RS2_STREAM_FISHEYE,1,Kl,Dl);
     getimageinst(profile,distparamr,RS2_STREAM_FISHEYE,2,Kr,Dr);
    
    while (1)
   {
      data = pipe.wait_for_frames();
      Mat srcl,dstl,srcr,dstr;
      srcl=GetImage(data,1);
      srcr=GetImage(data,2);
      

      
      undistort(srcl,dstl,Kl,Dl,Kl);
      Mat Knew=Mat::eye(3,3,CV_64F);
      //fisheye::undistortImage(srcl,dstl,Kl,Dl,Knew,srcl.size());

      cv::imshow("../left.png", dstl);
      cv::imshow("../right.png",srcr);
      cv::waitKey(500);
    }

    return 0;
}
